﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Drawing;

namespace Nate.Pathfinding
{
    public class AStar_Simple : Pathfinding.IPathfind
    {
        // 
        // Public Methods 
        //
        
        public void Init(NodeID start_id, NodeID dest_id, Graph graph)
        {
            _success = false;
            _graph = graph;
            _source = start_id.GraphIndex;
            _destination = dest_id.GraphIndex;

            Node _source_node = _graph.Nodes[_source];
            _destination_node = _graph.Nodes[_destination];

            _costs_g = NUtils.Array.AllocateWithDefault<double>(_graph.Nodes.Count, 0);
            _costs_h = NUtils.Array.AllocateWithDefault<double>(_graph.Nodes.Count, 0);
            _costs_f = NUtils.Array.AllocateWithDefault<double>(_graph.Nodes.Count, 0);

            _costs_h[_source] = _Heuristic(_source_node);
            _costs_f[_source] = _costs_h[_source];

            _came_from = new Edge[_graph.Nodes.Count];
            
            _closed = new List<Node>();
            _open = new List<Node>();
            _open.Add(_graph.Nodes[_source]);
        }

        public AStar_Simple()
        {
            _options = new Nate.Shared.Utils.Options();
            _options.Label = "Heuristics";
            _options.ToolTipText = "Available Heuristics";
            _options.Selections = new string[] { "Distance^2", /*"Distance",*/ "Manhattan" };
            _heuristic = Heuristics.DistanceSquared;
        }

        public Shared.Status Tick()
        {
            if (_success)
            {
                return Shared.Status.Success;
            }

            if (0 == _open.Count)
            {
                return Shared.Status.Failure;
            }

            return _tick();
        }

        // 
        // Public Properties 
        //

        public List<Point> Path
        {
            get
            {
                List<Point> points = new List<Point>();

                if (_success)
                {
                    // Give the caller the route that was found.

                    List<int> route = new List<int>();

                    int i = _destination;
                    while (i != _source)
                    {
                        route.Add(i);

                        Edge e = _came_from[i];
                        if (null == e)
                        {
                            break;
                        }

                        i = e.Other(i);
                    }

                    route.Reverse();

                    foreach (int step in route)
                    {
                        points.Add(_graph.Nodes[step].Point);
                    }

                }
                else
                {
                    // Let the caller see the nodes that we've visited.

                    foreach (Node n in _closed)
                    {
                        points.Add(n.Point);
                    }
                }

                return points;
            }
        }

        public Nate.Shared.Utils.Options Options
        {
            get
            {
                return _options;
            }
        }

        public string OptionSelection
        {
            set
            {
                if (value == Heuristics.Distance.ToString())
                {
                    _heuristic = Heuristics.Distance;
                }
                else if (value == Heuristics.Manhattan.ToString())
                {
                    _heuristic = Heuristics.Manhattan;
                }
                else
                {
                    _heuristic = Heuristics.DistanceSquared;
                }
            }
        }

        // 
        // Private Objects
        //

        enum Heuristics
        {
            DistanceSquared,
            Distance,
            Manhattan
        }

        // 
        // Private Data
        //

        private Heuristics _heuristic;
        private Shared.Utils.Options _options;
        private bool _success;
        private int _source;
        private int _destination;
        private Graph _graph;
        private Node _destination_node;
        private double[] _costs_g;
        private double[] _costs_h;
        private double[] _costs_f;
        private Edge[] _came_from;
        private List<Node> _open;
        private List<Node> _closed;

        //
        // Private Methods
        //

        private Shared.Status _tick()
        {
            _open.Sort(_CompareNodes);
            Node next_best = _open[0];
            int next_index = next_best.Index;

            if (_destination == next_index)
            {
                _success = true;
                return Shared.Status.Success;
            }

            _open.RemoveAt(0);
            _closed.Add(next_best);

            foreach (Edge e in next_best.Edges)
            {
                int neighbor_index = e.Other(next_index);
                Node neighbor_node = _graph.Nodes[neighbor_index];

                if (_closed.Contains(neighbor_node))
                {
                    continue;
                }

                double cost_g = _costs_g[next_index] + e.Cost;
                bool better = false;

                if (!_open.Contains(neighbor_node))
                {
                    _open.Add(neighbor_node);
                    _costs_h[neighbor_index] = _Heuristic(neighbor_node);
                    better = true;
                }
                else if (cost_g < _costs_g[neighbor_index])
                {
                    better = true;
                }

                if (better)
                {
                    _came_from[neighbor_index] = e;
                    _costs_g[neighbor_index] = cost_g;
                    _costs_f[neighbor_index] = _costs_g[neighbor_index] + _costs_h[neighbor_index];
                }
            }
            return Shared.Status.InProcess;
        }

        private int _CompareNodes(Node a, Node b)
        {
            double cost_a = _costs_f[a.Index];
            double cost_b = _costs_f[b.Index];
            int comp = cost_a.CompareTo(cost_b);
            return comp;
        }

        private double _Heuristic(Node n)
        {
            switch (_heuristic)
            {
                case Heuristics.DistanceSquared:
                    return _destination_node.DistanceSquared(n);
                case Heuristics.Manhattan:
                    return _destination_node.DistanceManhattan(n);
                case Heuristics.Distance:
                    return _destination_node.Distance(n);
            }

            return _destination_node.DistanceSquared(n);
        }
    }
}
